/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.Counter;
import edu.wpi.first.wpilibj.CounterBase;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;

/**
 *
 * @author Jared
 */
public class Shooter extends PIDSubsystem {

    private static final double Kp = 0.0;
    private static final double Ki = 0.0;
    private static final double Kd = 0.0;
    private static Shooter instance;
    public static final double SHOOTER_KEY_SPEED = 1900;
    public static final double SHOOTER_FENDER_SPEED = 1600;
    
    public static Shooter getInstance() {
        if(instance == null){
            instance = new Shooter();
        }
        return instance;
    }
   private final SpeedController shooter = new Victor(RobotMap.SHOOTER_MOTOR_PORT);
   private final CounterBase optical = new Counter(RobotMap.SHOOTER_OPTICAL_PORT);
   

    // Initialize your subsystem here
    public Shooter() {
        super("Shooter", Kp, Ki, Kd);
        setSetpointRange(0, 1);
        setSetpoint(0);
        enable();
        

        // Use these to get going:
        // setSetpoint() -  Sets where the PID controller should move the system
        //                  to
        // enable() - Enables the PID controller.       
    }
    
    public void shootFromKey(){
        setSetpoint(SHOOTER_KEY_SPEED);
    }
    
    public void shootFromFender(){
        setSetpoint(SHOOTER_FENDER_SPEED);
    }
    
    public void setShooterSpeed(double shooterSpeed) {
        setSetpoint(shooterSpeed);
    }
    
    public boolean shooterAtSetpoint(){
        return Math.abs(getPosition()-getSetpoint())<.1;
    }
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
    
    protected double returnPIDInput() {
        // Return your input value for the PID loop
        // e.g. a sensor, like a potentiometer:
        // yourPot.getAverageVoltage() / kYourMaxVoltage;
        return 1 / optical.getPeriod();
    }
    
    protected void usePIDOutput(double output) {
        
        // Use output to drive your system, like a motor
        // e.g. yourMotor.set(output);
    }
}
